#ifndef _PID_t_H__
#define _PID_t_H__

#include "math.h"

/*---PID结构体---*/
typedef struct
{
	float kp;								//< P
	float ki;								//< I
	float kd;								//< D

	float last_output;					//< 上一次输出值
//	float error;								//< 误差
	float last_error;          //< 上次误差
	float integral;						//< 误差积分
	
	float output_limit;
	float integral_limit;      //< 积分最大值

	float Derror;								//< 误差微分
	float last_Derror;					//< 上一次的误差微分
	float DDerror;							//< 误差微分的微分
}PID_t;

typedef struct
{
	PID_t in;
	PID_t out;
}PID_Gather_t;

typedef struct
{
	PID_t Right;
	PID_t Lift;
}PID_Brother_t;

typedef struct
{
	PID_t x;
	PID_t y;
}PID_Direction_t;

typedef struct
{
	PID_t m1;
	PID_t m2;
	PID_t m3;
	PID_t m4;
}PID_Motor_t;

#include "main.h"

void pid_init(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);
int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit);

float pid_calculate_position(PID_t *pid, const float set_value, const float now_value, float Dt);
float PID_Compute_Increm(PID_t *pid, float sp, float val);

float yaw_clamp( double control_yaw_error );
float yaw_pid_updata(PID_t *pid, float control_yaw, float now_yaw, float Dtime);

void pid_reset_integral(PID_t *pid);
void PID_Reset(PID_t * PID);
void PID_SetOutClamp(PID_t *hpid, float max, float min);

#endif


